//
// Created by bill3000 on 2/9/21.
// Every thing need to start fruit sorting kernel thread is to
// call FruitFSM::start_sorting_kernel_thread, then every thing is done
//

#include "fruit.hpp"
#include "../log/logger.hpp"

extern bool running;

FruitFSM *FruitFSM::fruit = nullptr;  //private static member
Queue *FruitFSM::mq = nullptr;  //private static member
pthread_t FruitFSM::sorting_kernel_thread_id;  //private static member

bool FruitFSM::learning = false;

cv::VideoCapture *FruitFSM::cam1 = nullptr;
cv::VideoCapture *FruitFSM::cam2 = nullptr;
cv::VideoCapture *FruitFSM::cam3 = nullptr;
cv::VideoCapture *FruitFSM::cam4 = nullptr;

SizeModel FruitFSM::learned_size_model[3] = {0};  //learned_size_model: learned size model
SizeModel FruitFSM::loaded_size_model[3] = {0};  //loaded_size_model: loaded size model
bool FruitFSM::size_model_loaded = false;
bool FruitFSM::sugar_model_loaded = false;
bool FruitFSM::sorting = false;
pthread_t FruitFSM::addon_sugar_thread_id = -1;
Statistics FruitFSM::statistics = {0};
std::map<long, float> FruitFSM::sugar_ann_nirid_to_sugar;
pthread_t FruitFSM::sugar_learning_thread_id = -1;
int FruitFSM::sugar_learning_progress_in_percent = 0;

void FruitFSM::start_sorting_kernel_thread(){
    int ret;
    fruit = new FruitFSM();
    fruit->make_fsm();
    fruit->make_actions();
    fruit->set_current(STATE_INIT);

    ret = pthread_create(&sorting_kernel_thread_id,
                         nullptr, sorting_kernel_thread, (void *)fruit);
    if(ret!=0) {
        LOG(ERROR, "Failed to create sorting kernel thread !");
        exit(1);
    }
}

void FruitFSM::wait_sorting_kernel_thread() {
    pthread_join(sorting_kernel_thread_id, nullptr);
}

void FruitFSM::stop_sorting_kernel_thread() {
    pthread_cancel(FruitFSM::sorting_kernel_thread_id);
}

void FruitFSM::dispose() {
    delete fruit;
    delete cam1; delete cam2; delete cam3; delete cam4;
    Queue::dispose();
}

void* FruitFSM::sorting_kernel_thread(void *data) {
    auto *f = (FruitFSM *)data;
    f->running_analysis();
    pthread_exit(nullptr);
}

void FruitFSM::running_analysis() {
    Message message;
    int ret;

    LOG(INFO, "Start sorting kernel thread successfully !");
    while(running) {
        ret = mq->receive_message(M02, &message, MESSAGE_DIRECTION_UI_THREAD_TO_SORTING_THREAD);
        if (ret == -1) {
            LOG(ERROR, strerror(errno));
            continue;
        }
        std::cout << "FruitFSM->runing_analysis receive message successful!" << std::endl;
        process(&(message.data));
    }
}

void FruitFSM::process(Event *event) {
    Transition *trans = current->get_transition(event->ctype);
    if (trans == nullptr) return;

    unsigned int exit_action = current->get_exit_action();
    unsigned int action_code = trans->get_action_code();
    State *newState = get_state(trans->get_state_code());
    if (newState == nullptr) {
        std::cout << "Destination state is not defined !"<<std::endl;
        LOG(ERROR, "Destination state is not defined !");
        return;
    }

    unsigned int entry_action = newState->get_entry_action();
    run(exit_action, event);
    run(action_code, event);
    run(entry_action, event);
}

int FruitFSM::run(unsigned int action_code, Event *event) {
    auto iter = actions.find(action_code);
    if(iter != actions.end()) {
        Action action = iter->second;
        return action(event);
    }
    LOG(ERROR, "Action is not defined !");
    return -1;
}

FruitFSM::FruitFSM() {
    mq = Queue::get_instance();
    current = nullptr;
}

FruitFSM::~FruitFSM() = default;

void FruitFSM::add_state(State& state) {
    machine.insert(std::pair<unsigned int, State>(state.get_state_code(), state));
    current = &machine.at(state.get_state_code());
}

void FruitFSM::add_transition(Transition& transition) {
    current->get_transitions().insert(
            std::pair<unsigned int, Transition>(transition.get_event_code(), transition));
}

int FruitFSM::set_current(unsigned int state_code) {
    State *p;
    if ((p = get_state(state_code)) != nullptr) {
        current = p;
        return 0;
    }
    LOG(ERROR, "New state is not valid !");
    return -1;
}

State* FruitFSM::get_state(unsigned int state_code) {
    auto iter = machine.find(state_code);
    if (iter != machine.end()) {
        return &(iter->second);
    }
    return nullptr;
}

void FruitFSM::make_fsm() {
    State st(STATE_INIT, "Init", ACTION_NONE, ACTION_OPEN_CAM);
    add_state(st);
    Transition tr(EVENT_GO_TO_NEXT_STATE_DIRECTLY, ACTION_NONE, STATE_READY);
    add_transition(tr);

    st = State(STATE_READY, "Ready", ACTION_NONE, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_GET_CAM_PARAM, ACTION_GET_CAM_PARAM, STATE_READY);
    add_transition(tr);
    tr = Transition(EVENT_PREVIEW_STILL_IMAGE, ACTION_PREVIEW_STILL_IMAGE, STATE_READY);
    add_transition(tr);
    tr = Transition(EVENT_PREVIEW_MOVING_IMAGE, ACTION_PREVIEW_MOVING_IMAGE, STATE_MOVING_IMAGE_PREVIEW_INIT);
    add_transition(tr);
    tr = Transition(EVENT_MODIFY_CAM_PARAM, ACTION_MODIFY_CAM_PARAM, STATE_READY);
    add_transition(tr);
    tr = Transition(EVENT_LOAD_SIZE_MODEL, ACTION_LOAD_SIZE_MODEL, STATE_READY);
    add_transition(tr);
    tr = Transition(EVENT_LOAD_SUGAR_MODEL, ACTION_LOAD_SUGAR_MODEL, STATE_READY);
    add_transition(tr);
    tr = Transition(EVENT_START_SIZE_AUTO, ACTION_START_SIZE_AUTO_LEARNING, STATE_SIZE_AUTO_LEARNING_SAL);
    add_transition(tr);
    tr = Transition(EVENT_START_SIZE_SEMI, ACTION_START_SEMI_SIZE_LEARNING, STATE_SIZE_SEMI_LEARNING_SSL);
    add_transition(tr);
    tr = Transition(EVENT_START_SUGAR_ANN, ACTION_START_SUGAR_ANNOTATION, STATE_SUGAR_ANNOTATION);
    add_transition(tr);
    tr = Transition(EVENT_START_SORTING, ACTION_START_SORTING, STATE_RUNNING_SORTING);
    add_transition(tr);
    tr = Transition(EVENT_GET_HISTORY_SORTED_LIST, ACTION_GET_HISTORY_SORTING_DATA, STATE_MANAGE_HISTORY_DATA);
    add_transition(tr);

    st = State(STATE_MOVING_IMAGE_PREVIEW_INIT, "Image Preview Init", ACTION_START_CONVEYOR, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_ANY, ACTION_PUT_EVENT_BACK_TO_QUEUE, STATE_MOVING_IMAGE_PREVIEW);
    add_transition(tr);

    st = State(STATE_MOVING_IMAGE_PREVIEW, "Image Preview", ACTION_NONE, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_PREVIEW_MOVING_IMAGE, ACTION_PREVIEW_MOVING_IMAGE, STATE_MOVING_IMAGE_PREVIEW);
    add_transition(tr);
    tr = Transition(EVENT_ANY, ACTION_PUT_EVENT_BACK_TO_QUEUE, STATE_MOVING_IMAGE_PREVIEW_END);
    add_transition(tr);

    st = State(STATE_MOVING_IMAGE_PREVIEW_END, "Image Preview End", ACTION_NONE, ACTION_STOP_CONVEYOR);
    add_state(st);
    tr = Transition(EVENT_ANY, ACTION_PUT_EVENT_BACK_TO_QUEUE, STATE_READY);
    add_transition(tr);

    st = State(STATE_SIZE_AUTO_LEARNING_SAL, "Size Auto Learning，SAL", ACTION_NONE, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_GET_SIZE_AUTO_PROCESS_IMAGE, ACTION_GET_SIZE_AUTO_PROGRESS_IMAGE, STATE_SIZE_AUTO_LEARNING_SAL);
    add_transition(tr);
    tr = Transition(EVENT_COMPLETE_SIZE_AUTO, ACTION_STOP_SIZE_AUTO_LEARNING, STATE_LEARNING_SAL_STOPPED);
    add_transition(tr);

    st = State(STATE_LEARNING_SAL_STOPPED, "SAL Stopped", ACTION_NONE, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_LEARNING_SAL_COMPLETED, ACTION_LEARNING_SAL_RETURN_LEARNED_RESULT_TO_UI, STATE_SAL_COMPLETE);
    add_transition(tr);

    st = State(STATE_SAL_COMPLETE, "SAL Complete", ACTION_NONE, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_VIEW_SIZE_SAMPLE, ACTION_VIEW_SIZE_LEARNING_SAMPLE, STATE_SAL_COMPLETE);
    add_transition(tr);
    tr = Transition(EVENT_SAVE_SIZE_LEARNED, ACTION_SAVE_SIZE_LEARNING_MODEL, STATE_SAL_COMPLETE);
    add_transition(tr);
    tr = Transition(EVENT_ANY, ACTION_PUT_EVENT_BACK_TO_QUEUE, STATE_READY);
    add_transition(tr);

    st = State(STATE_SIZE_SEMI_LEARNING_SSL, "Size Semi Learning，SSL", ACTION_NONE, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_COMPLETE_SIZE_SEMI, ACTION_STOP_SEMI_SIZE_LEARNING, STATE_LEARNING_SSL_STOPPED);
    add_transition(tr);

    st = State(STATE_LEARNING_SSL_STOPPED, "SSL stopped", ACTION_NONE, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_LEARNING_SSL_COMPLETED, ACTION_LEARNING_SSL_RETURN_LEARNED_RESULT_TO_UI, STATE_SSL_COMPLETE);
    add_transition(tr);

    st = State(STATE_SSL_COMPLETE, "SSL Complete", ACTION_NONE, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_START_SIZE_SEMI, ACTION_START_SEMI_SIZE_LEARNING, STATE_SIZE_SEMI_LEARNING_SSL);
    add_transition(tr);
    tr = Transition(EVENT_VIEW_SIZE_SAMPLE, ACTION_VIEW_SIZE_LEARNING_SAMPLE, STATE_SSL_COMPLETE);
    add_transition(tr);
    tr = Transition(EVENT_SAVE_SIZE_LEARNED, ACTION_SAVE_SIZE_LEARNING_MODEL, STATE_SSL_COMPLETE);
    add_transition(tr);
    tr = Transition(EVENT_ANY, ACTION_PUT_EVENT_BACK_TO_QUEUE, STATE_READY);
    add_transition(tr);

    st = State(STATE_SUGAR_ANNOTATION, "Sugar Annotation, SA", ACTION_NONE, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_SUGAR_NEXT_PREVIOUS_VALUE, ACTION_GET_NEXT_NIR_IMAGE_PREVIOUS_SUGAR, STATE_SUGAR_ANNOTATION);
    add_transition(tr);
    tr = Transition(EVENT_STOP_SUGAR_ANN, ACTION_STOP_SUGAR_ANNOTATION, STATE_SA_COMPLETE);
    add_transition(tr);

    st = State(STATE_SA_COMPLETE, "SA Complete", ACTION_NONE, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_SAVE_SUGAR_ANN, ACTION_SAVE_SUGAR_ANNOTATION, STATE_SA_COMPLETE);
    add_transition(tr);
    tr = Transition(EVENT_START_SUGAR_LEARNING, ACTION_START_SUGAR_LEARNING, STATE_SUGAR_LEARNING);
    add_transition(tr);

    st = State(STATE_SUGAR_LEARNING, "Sugar Learning", ACTION_NONE, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_SUGAR_QUERY_LEARNING_PROGRESS, ACTION_QUERY_SUGAR_LEARNING_PROGRESS, STATE_SUGAR_LEARNING);
    add_transition(tr);
    tr = Transition(EVENT_SAVE_SUGAR_LEARNED, ACTION_SAVE_SUGAR_LEARNED_MODEL, STATE_SUGAR_LEARNING);
    add_transition(tr);
    tr = Transition(EVENT_CANCEL_SUGAR_LEARNING, ACTION_CANCEL_SUGAR_LEARNING, STATE_SUGAR_LEARNING);
    add_transition(tr);
    tr = Transition(EVENT_ANY, ACTION_PUT_EVENT_BACK_TO_QUEUE, STATE_READY);
    add_transition(tr);

    st = State(STATE_RUNNING_SORTING, "Running Sorting", ACTION_NONE, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_VIEW_SORTING_PRECESS_IMAGE, ACTION_VIEW_SORTING_PROCESS_IMAGE, STATE_RUNNING_SORTING);
    add_transition(tr);
    tr = Transition(EVENT_VIEW_SORTING_STATISTICS, ACTION_VIEW_SORTING_STATISTICS, STATE_RUNNING_SORTING);
    add_transition(tr);
    tr = Transition(EVENT_STOP_SORTING, ACTION_PAUSE_SORTING, STATE_PAUSE_SORTING);
    add_transition(tr);

    st = State(STATE_PAUSE_SORTING, "Pause Sorting", ACTION_NONE, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_VIEW_SORTING_STATISTICS, ACTION_VIEW_SORTING_STATISTICS, STATE_PAUSE_SORTING);
    add_transition(tr);
    tr = Transition(EVENT_START_SORTING, ACTION_START_SORTING, STATE_RUNNING_SORTING);
    add_transition(tr);
    tr = Transition(EVENT_ANY, ACTION_PUT_EVENT_BACK_TO_QUEUE, STATE_READY);
    add_transition(tr);

    st = State(STATE_MANAGE_HISTORY_DATA, "Manage History Data", ACTION_NONE, ACTION_NONE);
    add_state(st);
    tr = Transition(EVENT_VIEW_HISTORY_STATISTICS, ACTION_VIEW_HISTORY_STATISTICS, STATE_MANAGE_HISTORY_DATA);
    add_transition(tr);
    tr = Transition(EVENT_DELETE_HISTORY, ACTION_DELETE_HISTORY_DATA, STATE_MANAGE_HISTORY_DATA);
    add_transition(tr);
    tr = Transition(EVENT_ANY, ACTION_PUT_EVENT_BACK_TO_QUEUE, STATE_READY);
    add_transition(tr);
}

void FruitFSM::make_actions() {
    actions.insert(std::pair<unsigned int, Action>(ACTION_NONE, action_none));

    actions.insert(std::pair<unsigned int, Action>(ACTION_OPEN_CAM, action_open_cam));
    actions.insert(std::pair<unsigned int, Action>(ACTION_START_CONVEYOR, action_start_conveyor));
    actions.insert(std::pair<unsigned int, Action>(ACTION_STOP_CONVEYOR, action_stop_conveyor));

    actions.insert(std::pair<unsigned int, Action>(ACTION_GET_CAM_PARAM,  action_get_cam_param));
    actions.insert(std::pair<unsigned int, Action>(ACTION_PREVIEW_STILL_IMAGE, action_preview_still_image));
    actions.insert(std::pair<unsigned int, Action>(ACTION_PREVIEW_MOVING_IMAGE,  action_preview_moving_image));
    actions.insert(std::pair<unsigned int, Action>(ACTION_MODIFY_CAM_PARAM, action_modify_cam_param));

    actions.insert(std::pair<unsigned int, Action>(ACTION_START_SIZE_AUTO_LEARNING, action_start_size_auto_learning));
    actions.insert(std::pair<unsigned int, Action>(ACTION_GET_SIZE_AUTO_PROGRESS_IMAGE, action_get_size_auto_progress_image));
    actions.insert(std::pair<unsigned int, Action>(ACTION_STOP_SIZE_AUTO_LEARNING, action_stop_size_auto_learning));
    actions.insert(std::pair<unsigned int, Action>(ACTION_VIEW_SIZE_LEARNING_SAMPLE, action_view_size_learning_sample));
    actions.insert(std::pair<unsigned int, Action>(ACTION_SAVE_SIZE_LEARNING_MODEL,  action_save_size_learning_model));
    actions.insert(std::pair<unsigned int, Action>(ACTION_START_SEMI_SIZE_LEARNING,  action_start_semi_size_learning));
    actions.insert(std::pair<unsigned int, Action>(ACTION_STOP_SEMI_SIZE_LEARNING, action_stop_semi_size_learning));

    actions.insert(std::pair<unsigned int, Action>(ACTION_START_SUGAR_ANNOTATION, action_start_sugar_annotation));
    actions.insert(std::pair<unsigned int, Action>(ACTION_GET_NEXT_NIR_IMAGE_PREVIOUS_SUGAR,
                                                   action_get_next_nir_image_and_set_previous_nir_sugar));
    actions.insert(std::pair<unsigned int, Action>(ACTION_STOP_SUGAR_ANNOTATION, action_stop_sugar_annotation));
    actions.insert(std::pair<unsigned int, Action>(ACTION_SAVE_SUGAR_ANNOTATION, action_save_sugar_annotation));
    actions.insert(std::pair<unsigned int, Action>(ACTION_START_SUGAR_LEARNING, action_start_sugar_learning));
    actions.insert(std::pair<unsigned int, Action>(ACTION_QUERY_SUGAR_LEARNING_PROGRESS,  action_query_sugar_learning_progress));
    actions.insert(std::pair<unsigned int, Action>(ACTION_SAVE_SUGAR_LEARNED_MODEL, action_save_sugar_learned_model));
    actions.insert(std::pair<unsigned int, Action>(ACTION_CANCEL_SUGAR_LEARNING, action_cancel_sugar_learning));

    actions.insert(std::pair<unsigned int, Action>(ACTION_LOAD_SIZE_MODEL, action_load_size_model));
    actions.insert(std::pair<unsigned int, Action>(ACTION_LOAD_SUGAR_MODEL,  action_load_sugar_model));
    actions.insert(std::pair<unsigned int, Action>(ACTION_START_SORTING, action_start_sorting));
    actions.insert(std::pair<unsigned int, Action>(ACTION_VIEW_SORTING_PROCESS_IMAGE, action_view_sorting_process_image));
    actions.insert(std::pair<unsigned int, Action>(ACTION_VIEW_SORTING_STATISTICS, action_view_sorting_statistics));
    actions.insert(std::pair<unsigned int, Action>(ACTION_PAUSE_SORTING, action_pause_sorting));

    actions.insert(std::pair<unsigned int, Action>(ACTION_GET_HISTORY_SORTING_DATA, action_get_history_sorting_data));
    actions.insert(std::pair<unsigned int, Action>(ACTION_VIEW_HISTORY_STATISTICS, action_view_history_statistics));
    actions.insert(std::pair<unsigned int, Action>(ACTION_DELETE_HISTORY_DATA, action_delete_history_data));

    actions.insert(std::pair<unsigned int, Action>(ACTION_PUT_EVENT_BACK_TO_QUEUE, action_put_event_back_to_queue));
    actions.insert(std::pair<unsigned int, Action>(ACTION_LEARNING_SAL_RETURN_LEARNED_RESULT_TO_UI, action_learning_sal_return_learned_result_to_ui));
    actions.insert(std::pair<unsigned int, Action>(ACTION_LEARNING_SSL_RETURN_LEARNED_RESULT_TO_UI, action_learning_ssl_return_learned_result_to_ui));
}

